//	--------------------------------------------------------------------
//	Copyright(C) 2006,2007 Zhao Yukun. All rights reserved.
//	This file is a part of the Xeres system.
//	Xeres 2007
//	Contact : xeres.engine@gmail.com
//	$(reservedInfo)
//	$(reservedDeclaration)
//	--------------------------------------------------------------------
///	@file	<quat.h>
///	@path	~/src/lib/math/
///	@date	2007/09/16
///	@desc	Quaternion, for arbitrary space rotation.

#pragma once

#include "xeres.h"

#include "lib/math/math_ext.h"
#include "lib/math/vector.h"
#include "lib/math/matrix.h"

namespace xeres {

	/*!
		\class		Quat
		\brief		quaternion class.
	*/
	template<
		typename _Ty
	> class Quat : public ArithmeticOp< _Ty >
	{
	public:

		// scalar type
		typedef _Ty _Scalar;

		//@ data
		union {

			// coordinate system access
			struct { _Scalar x , y , z , real; };
			// array access
			_Scalar f[4];
		};

		// accessible member
		static const size_t DIMENSION = 4;

	public:
		/// \ctor
		Quat( void )
			: x(0) , y(0) , z(0) , real(1)
		{
		}

		/// \ctor_set
		template<
			typename _T1 , typename _T2 , typename _T3 , typename _T4
		> Quat( _T1 i , _T2 j , _T3 k , _T4 s )
			: x(_Scalar(i)) , y(_Scalar(j)) , z(_Scalar(k)) , real(_Scalar(s))
		{
		}

		/// \ctor
		template<
			typename _T1 , typename _T2
		> Quat( const Vector<_T1>& v , const _T2& r )
			: x(_Scalar(v.x)) , y(_Scalar(v.y)) , z(_Scalar(v.z)) , real(_Scalar(r))
		{
		}

		/// \ctor_copy
		Quat( const Quat& q )
			: x(q.x) , y(q.y) , z(q.z) , real(q.real)
		{
		}

		/// \ctor
		template<
			typename _TOther
		> Quat( const Quat<_TOther>& q )
			: x(_Ty(q.x)) , y(_Ty(q.y)) , z(_Ty(q.z)) , real(_Ty(q.real))
		{
		}

		// set from 4 any-type-values
		template<
			typename _T1 , typename _T2 , typename _T3 , typename _T4
		> void set( _T1 i , _T2 j , _T3 k , _T4 s )
		{
			x = _Scalar(i);
			y = _Scalar(j);
			z = _Scalar(k);
			real = _Scalar(s);
		}
		// set from a data collection with subscription
		template< typename _Ts > void set( const _Ts& sf )
		{
			x = _Scalar(sf[0]);
			y = _Scalar(sf[1]);
			z = _Scalar(sf[2]);
			real = _Scalar(sf[3]);
		}
		// memory copy from a floating point array
		void copy( const _Scalar * s )
		{
			memcpy( f , s , sizeof(_Scalar) * DIMENSION );
		}
		// set to identity
		void identity( void )
		{
			x = y = z = 0;
			real = 1;
		}
		// get identity
		static const Quat<_Scalar>& _identity( void )
		{
			static Quat<_Scalar> s_identityQuat( 0 , 0 , 0 , 1 );
			return s_identityQuat;
		}
		// set axis & angle
		template<
			typename _TOther
		> void set( const Vector<_TOther>& ax , const _TOther& rad )
		{
			real = cos( _Scalar(rad) * _Scalar(0.5) );
			Vector<_Scalar> n( ax );
			n.normalize();
			_Scalar s = sin( _Scalar(rad) * _Scalar(0.5) );
			x = n.x * s;
			y = n.y * s;
			z = n.z * s;
		}
		// square
		_Scalar square( void ) const
		{
			return real * real + dot3( &x , &x );
		}
		// length
		_Scalar length( void ) const
		{
			return sqrt( square() );
		}
		// normalize
		void normalize( void )
		{
			_Scalar len = square();
			if( is_zero( len ) )
			{
				identity();
			}
			else
			{
				len = recip_sqrt( len );
				real *= len;
				x *= len;
				y *= len;
				z *= len;
			}
		}
		Quat normalized( void )
		{
			_Scalar len = square();
			if( is_zero( len ) )
			{
				return Quat();
			}
			else
			{
				len = recip_sqrt( len );
				return Quat( x * len , y * len , z * len , real * len );
			}
		}
		// angle
		_Scalar angle( void ) const
		{
			return _Scalar(2.0) * acos( real );
		}
		// set angle
		void set_angle( _Scalar rad )
		{
			real = cos( rad * _Scalar(0.5) );
		}
		// axis
		Vector<_Scalar> axis( void ) const
		{
			Vector<_Scalar> result( x , y , z );
			return result.normalized();
		}
		// set axis
		void set_axis( const Vector<_Scalar>& a )
		{
			Vector<_Scalar> n = a.normalized();
			x = n.x;
			y = n.y;
			z = n.z;
		}
		// to matrix
		Matrix<_Scalar> to_matrix( void ) const
		{
			return Matrix<_Scalar>(
				_Scalar(1. - 2.0 * (y * y + z * z)),
				_Scalar(2.0 * (x * y + z * real)),
				_Scalar(2.0 * (z * x - y * real)),
				_Scalar(0.),
				_Scalar(2.0 * (x * y - z * real)),
				_Scalar(1. - 2.0 * (z * z + x * x)),
				_Scalar(2.0 * (y * z + x * real)),
				_Scalar(0.),
				_Scalar(2.0 * (z * x + y * real)),
				_Scalar(2.0 * (y * z - x * real)),
				_Scalar(1. - 2.0 * (y * y + x * x)),
				_Scalar(0.),
				_Scalar(0.),
				_Scalar(0.),
				_Scalar(0.),
				_Scalar(1.0) );
		}

		// dot product
		_Scalar dot( const Quat& to ) const
		{
			return real * to.real + dot3( &x , &(to.x) );
		}
		// inverse
		// 1    Q*
		// - = ----   where Q* is conjugate (operator~)
		// Q   Q* Q   and (Q* Q) == Q ^ Q (4D dot)
		Quat inverse( void ) const
		{
			_Scalar qdot = recip( dot( *this ) );
			return Quat( -x * qdot , -y * qdot , -z * qdot , real * qdot );
		}
		void invert( void )
		{
			_Scalar qdot = recip( dot( *this ) );
			real *= qdot;
			x *= -qdot;
			y *= -qdot;
			z *= -qdot;
		}
		// For unit quaternion, from Advanced Animation and 
		// Rendering Techniques by Watt and Watt, Page 366:
		Quat log( void ) const
		{
			_Scalar theta = acos( min( real, _Scalar(1.0) ) );
			if( theta == 0 )
				return Quat( x , y , z , 0 );

			_Scalar sintheta = sin( theta );

			_Scalar k;
			if( abs(sintheta) < 1 && abs(theta) >= infinite() * abs (sintheta) )
				k = 0;
			else
				k = theta / sintheta;

			return Quat( x * k, y * k, z * k , _Scalar(0) );
		}
		// For pure quaternion (zero scalar part):
		// from Advanced Animation and Rendering
		// Techniques by Watt and Watt, Page 366:
		Quat exp( void ) const
		{
			_Scalar theta = length3( &x );
			_Scalar sintheta = sin( theta );

			_Scalar k;
			if( abs(theta) < 1 && abs(sintheta) >= infinite() * abs(theta) )
				k = 0;
			else
				k = sintheta / theta;

			_Scalar costheta = cos(theta);

			return Quat( x * k , y * k , z * k , costheta );
		}
		//
		// Compute the angle between two quaternions,
		// interpreting the quaternions as 4D vectors.
		//
		static _Scalar angle4D( const Quat& q1 , const Quat& q2 )
		{
			Quat d = q1 - q2;
			_Scalar lengthD = sqrt( d.dot( d ) );

			Quat s = q2 + q2;
			_Scalar lengthS = sqrt( s.dot( s ) );

			return 2 * atan2( lengthD , lengthS );
		}
		//
		// Spherical linear interpolation.
		// Assumes q1 and q2 are normalized and that q1 != -q2.
		//
		// This method does *not* interpolate along the shortest
		// arc between q1 and q2.  If you desire interpolation
		// along the shortest arc, and q1^q2 is negative, then
		// consider flipping the second quaternion explicitly.
		//
		// The implementation of squad() depends on a slerp()
		// that interpolates as is, without the automatic
		// flipping.
		//
		// Don Hatch explains the method we use here on his
		// web page, The Right Way to Calculate Stuff, at
		// http://www.plunk.org/~hatch/rightway.php
		//
		static Quat slerp( const Quat& q1 , const Quat& q2 , const _Scalar t )
		{
			_Scalar a = angle4D( q1 , q2 );
			_Scalar s = 1 - t;
			Quat q = q1 * ( sinx_over_x( s * a ) / sinx_over_x ( a ) * s )  +
				q2 * ( sinx_over_x( t * a ) / sinx_over_x( a ) * t );
			return q.normalized();
		}
		// Spherical Cubic Spline Interpolation -
		// from Advanced Animation and Rendering
		// Techniques by Watt and Watt, Page 366:
		// A spherical curve is constructed using three
		// spherical linear interpolations of a quadrangle
		// of unit quaternions: q1, qa, qb, q2.
		// Given a set of quaternion keys: q0, q1, q2, q3,
		// this routine does the interpolation between
		// q1 and q2 by constructing two intermediate
		// quaternions: qa and qb. The qa and qb are 
		// computed by the intermediate function to 
		// guarantee the continuity of tangents across
		// adjacent cubic segments. The qa represents in-tangent
		// for q1 and the qb represents the out-tangent for q2.
		// 
		// The q1 q2 is the cubic segment being interpolated. 
		// The q0 is from the previous adjacent segment and q3 is 
		// from the next adjacent segment. The q0 and q3 are used
		// in computing qa and qb.
		static Quat spline(
			const Quat& q0, const Quat& q1,
			const Quat& q2, const Quat& q3, const _Scalar t)
		{
			Quat qa = intermediate (q0, q1, q2);
			Quat qb = intermediate (q1, q2, q3);
			Quat result = squad(q1, qa, qb, q2, t);
			return result;
		}
		// From advanced Animation and Rendering
		// Techniques by Watt and Watt, Page 366:
		// computing the inner quadrangle 
		// points (qa and qb) to guarantee tangent
		// continuity.
		static Quat intermediate(const Quat& q0, const Quat& q1, const Quat& q2)
		{
			// From advanced Animation and Rendering
			// Techniques by Watt and Watt, Page 366:
			// computing the inner quadrangle 
			// points (qa and qb) to guarantee tangent
			// continuity.
			// 
			Quat q1inv = q1.inverse();
			Quat c1 = q1inv*q2;
			Quat c2 = q1inv*q0;
			Quat c3 = (c2.log() + c1.log()) * _Scalar(-0.25);
			Quat qa = q1 * c3.exp();
			qa.normalize();
			return qa;
		}
		// Spherical Quadrangle Interpolation -
		// from Advanced Animation and Rendering
		// Techniques by Watt and Watt, Page 366:
		// It constructs a spherical cubic interpolation as 
		// a series of three spherical linear interpolations 
		// of a quadrangle of unit quaternions. 
		static Quat squad(const Quat &q1 , const Quat &qa , const Quat &qb , const Quat &q2 , const _Scalar& t )
		{
			Quat r1 = slerp(q1, q2, t);
			Quat r2 = slerp(qa, qb, t);
			Quat result = slerp(r1, r2, 2*t*(1-t));
			return result;
		}

		void set_rotation( const Vector<_Scalar>& from, const Vector<_Scalar>& to )
		{
			Vector<_Scalar> v;
			_Scalar cost = from.dot(to) / sqrt( from.dot(from) * to.dot(to) );

			// check for degeneracies
			if( cost > 0.99999 )
			{
				// Vectors are parallel.
				real = 1.0;
				x = y = z = 0;
			}
			else if( cost < -0.99999 )
			{
				// Vectors are opposite. Find an axis to rotate around,
				// which should be perpendicular to the original axis.
				Vector<_Scalar> frm = from.normalized();
				v.cross( frm , Vector<_Scalar>( 1 , 0 , 0 ) );

				if( v.length() < 0.00001 )
					v.cross( frm , Vector<_Scalar>( 0 , 1 , 0 ) );
				real = 0;
				v.normalize();
				x = v.x; y = v.y; z = v.z;
			}
			else
			{
				// Use half-angle formulae:
				// cos^2 t = ( 1 + cos (2t) ) / 2
				// w part is cosine of half the rotation angle

				real = (_Scalar)sqrt( 0.5 * ( 1.0 + cost ) );

				// sin^2 t = ( 1 - cos (2t) ) / 2
				// Do the normalization of the axis vector at the same time so
				// we only call sqrt once.
				v.cross( from , to );
				v *= (_Scalar)sqrt( ( 0.5 * ( 1.0 - cost ) ) / ( v.dot( v ) ) );
				x = v.x; y = v.y; z = v.z;
			}
		}


		// assignment
		const Quat& operator = ( const Quat& a ) {
			x = a.x;
			y = a.y;
			z = a.z;
			real = a.real;
			return *this;
		}
		template< typename _TOther > const Quat<_Scalar>& operator = ( const Quat<_TOther>& a )
		{
			x = _Scalar(a.x);
			y = _Scalar(a.y);
			z = _Scalar(a.z);
			real = _Scalar(a.real);
			return *this;
		}

		const Quat& operator += ( const Quat& op )
		{
			x += _Scalar(op.x);
			y += _Scalar(op.y);
			z += _Scalar(op.z);
			real += _Scalar(op.real);
			return *this;
		}
		const Quat& operator -= ( const Quat& op )
		{
			x -= _Scalar(op.x);
			y -= _Scalar(op.y);
			z -= _Scalar(op.z);
			real -= _Scalar(op.real);
			return *this;
		}
		const Quat& operator *= ( const Quat& op )
		{
			_Scalar rtmp = real * op.real - dot3( &x , &op.x );
			Vector<_Scalar> v1( x , y , z ) , v2( op.x , op.y , op.z );
			v2.cross( v2 , v1 );
			v1 = v1 * real + v1 * op.real + v2;
			x = v1.x;
			y = v1.y;
			z = v1.z;
			real = rtmp;
			return *this;
		}
		const Quat& operator *= ( const _Scalar& op )
		{
			real *= _Scalar(op);
			x *= _Scalar(op);
			y *= _Scalar(op);
			z *= _Scalar(op);
			return *this;
		}
		const Quat& operator /= ( const Quat& op )
		{
			*this = *this * op.inverse();
			return *this;
		}
		const Quat& operator /= ( const _Scalar& op )
		{
			_Scalar r = _Scalar(recip( op ));
			real *= r;
			x *= r;
			y *= r;
			z *= r;
			return *this;
		}

		// outside operators
		Quat operator - ( void ) const
		{
			return Quat( -x , -y , -z , -real );
		}
		Quat operator + ( const Quat& b ) const
		{
			return Quat( x + b.x , y + b.y , z + b.z , real + b.real );
		}
		Quat operator + ( const _Scalar& b ) const
		{
			return Quat( x + b , y + b , z + b , real + b );
		}
		Quat operator - ( const Quat& b ) const
		{
			return Quat( x - b.x , y - b.y , z - b.z , real - b.real );
		}
		Quat operator - ( const _Scalar& b ) const
		{
			return Quat( x - b , y - b , z - b , real - b );
		}
		Quat operator * ( const Quat& b ) const
		{
			_Scalar rtmp = real * b.real - dot3( &x , &b.x );
			Vector<_Scalar> v1( x , y , z ) , v2( b.x , b.y , b.z );
			v2.cross( v2 , v1 );
			v1 = v1 * real + v1 * b.real + v2;
			return Quat( v1.x , v1.y , v1.z , rtmp );
		}
		Quat operator * ( const _Scalar& b ) const
		{
			Vector<_Scalar> v( x , y , z );
			return Quat( v * b , real * b );
		}
		Quat operator / ( const Quat& b ) const
		{
			return (*this) * b.inverse();
		}
		Quat operator / ( const _Scalar& b ) const
		{
			_Scalar r = recip( b );
			return Quat( x * r , y * r , z * r , real * r );
		}
	};

} // namespace xeres
